#include <cstdio>


#include "DeformationState.h"
#include "IncrementalForceIteration.h"
#include "FEMModel.h"
#include "LinearCGState.h"
#include "BigVector.h"
#include "Setting.h"

/*
Incremental force iteration.

This method of used as a predictor for predictor-corrector type of
FE iterations.

Doesn't work very well:

* lots of parameters to tune (either manually or automatically),
such as the length of the force-steps.

* inefficient compared to the NL-CG or Tensor-Mass.

* jerky motion.

Did not investigate more thoroughly.

*/
IncrementalForceIteration::IncrementalForceIteration (DeformationState*pstate)
{
	pstate_ = pstate;
	cg_state_ = new LinearCGState;
	cg_state_-> residual_tolerance_ = GetNumberSetting ("incremental-toleran");
	pstate->incremental_b_ = true;
}

static void ComputeIncrementalGradient (void *info, Real * force, Real const *dis)
{
	IncrementalForceIteration * me = (IncrementalForceIteration*)info;

	me->pstate_->ApplyForceDerivativeAndResidual (force,
		0,
		me->pstate_->displacement_.AccessArray(),
		dis);
	BigVectorNegate (force, force, me->Dimension());
}

bool IncrementalForceIteration::GoodSolution (DeformationState const* )const
{
	Real todolen = BigVectorLength (todo_delta_force_.AccessArray (),
		Dimension ());

	printf ("todolen: %f\n" ,todolen);

	return (todolen < 1e-3);
}
int
IncrementalForceIteration::Dimension ()const
{
	return current_delta_displ_.Size ();
}


void IncrementalForceIteration::IterationStep (DeformationState *)
{
	Real *du = current_delta_displ_ .UnsafeAccessArray ();
	Real *df = current_delta_displ_.UnsafeAccessArray ();
	Real *res = cg_state_->residual_.UnsafeAccessArray();
	Real *todo = todo_delta_force_.UnsafeAccessArray();
	int n =Dimension();
	if ( !cg_state_->CGGoodSolution (current_df_len_))
	{
		cg_state_->CGIterationStep (du,
			pstate_->fixed_arr_.AccessArray (),
			(void*) this,
			&ComputeIncrementalGradient);
		cg_state_->prev_residual_len_ = cg_state_->residual_len_;
		cg_state_->residual_len_ = BigVectorIP (res,res,n);
		iter_count_ ++;
		return ;
	}
	else if (iter_count_)
	{
		BigVectorAdd (pstate_->displacement_.UnsafeAccessArray (),
			du,
			pstate_->displacement_.UnsafeAccessArray (),
			n);
		BigVectorSubtract (todo, todo, df,n);
		iter_count_ = 0;

		pstate_->UpdateIncrementalStep ();
		return ;
	}


	Real amount = 0.01;
	Real todo_len =  BigVectorLength (todo, n);
	if (todo_len <  amount)
	{
		todo_delta_force_ = current_delta_force_; 
	}
	else
	{
		BigVectorScale (df, amount/ todo_len, todo, n);
		BigVectorScale (todo, 1 - amount /todo_len, todo,n );
	}

	current_df_len_ = BigVectorIP (df, df,n );
	cg_state_->CGReinit ();
	BigVectorCopy (res, df,n);
	cg_state_->residual_len_ = BigVectorIP (res,res,n);
}

LinkArray< Array<Real> > IncrementalForceIteration::VectorVariables ()
{
	Array<Real> *vecs[]
	= {
		&todo_delta_force_, &current_delta_force_, &current_delta_displ_, 0};

		LinkArray< Array<Real> > vvs;
		for (int i= 0; vecs[i]; i++)
			vvs.Push (vecs[i]);

		vvs.Concat (cg_state_->VectorVariables());
		return vvs;
}

void IncrementalForceIteration::Reinit (DeformationState const *ps)
{
	IterationState::Reinit (ps);
	int n = Dimension();
	BigVectorNullify (current_delta_displ_.UnsafeAccessArray(),n);
	BigVectorNullify (current_delta_force_.UnsafeAccessArray(),n);
	cg_state_->CGReinit ();
}

void IncrementalForceIteration::UpdateResidual (DeformationState const*pstate)
{
	int n = Dimension ();
	BigVectorAdd (todo_delta_force_.UnsafeAccessArray (),
		pstate->delta_external_force_.AccessArray(),
		todo_delta_force_.AccessArray(),
		n);

}

IncrementalForceIteration::~IncrementalForceIteration()
{

}
